# 导入必要的库
import serial  # 用于串口通信
import time  # 用于时间延迟
import signal  # 用于处理信号（如Ctrl+C）
import struct  # 用于解析二进制数据

# 定义一个类来读取XJC传感器的数据
class XJCSensorReader:
    def __init__(self, port, baudrate=115200, sensor_type="D80"):
        # 初始化串口对象
        self.ser = serial.Serial()
        # 设置串口端口号
        self.ser.port = port
        # 设置波特率，默认为115200
        self.ser.baudrate = baudrate
        # 设置超时时间为0.1秒，非阻塞读取
        self.ser.timeout = 0.1  
        # 设置传感器类型，默认为"D80"
        self.sensor_type = sensor_type
        # 标记采集是否正在运行，默认为False
        self.running = False
        
        # 根据传感器型号设置命令（示例命令，需根据实际协议调整）
        # self.command = bytes([0x01, 0x10, 0x01, 0x9A, 0x00, 0x01, 0x02, 0x00, 0x03, 0xEB, 0x6B])
        self.command = bytes([0x01, 0x10, 0x01, 0x9A, 0x00, 0x01, 0x02, 0x00, 0x00, 0xAB, 0x6A])

    def _parse_data(self, data):
        """解析传感器数据帧"""
        # 检查数据长度是否至少为16字节，并且帧头是否正确（0x20 0x4E）
        # if len(data) < 16 or data[0] != 0x20 or data[1] != 0x4E:
        #     return None
        
        try:
            # 解析力值（大端有符号short），单位转换为N
            fx = struct.unpack('>h', data[2:4])[0] / 100.0
            fy = struct.unpack('>h', data[4:6])[0] / 100.0
            fz = struct.unpack('>h', data[6:8])[0] / 100.0
            
            # 解析力矩值，单位转换为Nm
            mx = struct.unpack('>h', data[8:10])[0] / 1000.0
            my = struct.unpack('>h', data[10:12])[0] / 1000.0
            mz = struct.unpack('>h', data[12:14])[0] / 1000.0
            
            # 返回解析后的力和力矩值
            return (fx, fy, fz, mx, my, mz)
        except Exception as e:
            # 如果解析失败，返回None
            print(f"解析数据失败: {e}")
            return None

    def start(self):
        """启动数据采集"""
        # 如果串口未打开，则打开串口
        if not self.ser.is_open:
            self.ser.open()
            print(f"已打开串口 {self.ser.name}")
        
        # 标记采集开始
        self.running = True
        # 创建一个字节数组用于存储接收到的数据
        buffer = bytearray()
        
        # 发送初始化命令到传感器
        self.ser.write(self.command)
        
        try:
            # 主循环，持续采集数据直到停止
            while self.running:
                # 读取所有可用的数据并添加到缓冲区
                buffer += self.ser.read_all()
                
                # 查找帧头（0x20 0x4E）
                while len(buffer) >= 16:
                    header_pos = buffer.find(b'\x20\x4E')
                    if header_pos == -1:
                        # 如果找不到帧头，清空缓冲区
                        buffer.clear()
                        break
                    
                    # 如果帧头不在起始位置，丢弃之前的无效数据
                    if header_pos > 0:
                        buffer = buffer[header_pos:]
                    
                    # 如果剩余数据不足16字节，继续等待更多数据
                    if len(buffer) < 16:
                        break
                    
                    # 提取一帧完整的16字节数据
                    frame = bytes(buffer[:16])
                    buffer = buffer[16:]
                    
                    # 解析提取的数据帧
                    values = self._parse_data(frame)
                    if values:
                        # 打印解析后的力和力矩值
                        print(f"Fx: {values[0]:+.3f}N | Fy: {values[1]:+.3f}N | Fz: {values[2]:+.3f}N | "
                              f"Mx: {values[3]:+.3f}Nm | My: {values[4]:+.3f}Nm | Mz: {values[5]:+.3f}Nm")
                    
                # 短暂休眠以降低CPU占用
                time.sleep(0.01)

        except KeyboardInterrupt:
            # 捕获Ctrl+C信号，调用停止方法
            self.stop()
            
    def stop(self):
        """停止采集"""
        # 标记采集结束
        self.running = False
        # 如果串口已打开，则关闭串口
        if self.ser.is_open:
            self.ser.close()
            print("串口已关闭")

def signal_handler(sig, frame):
    # 处理Ctrl+C信号，打印提示信息并停止采集
    print("\n检测到Ctrl+C，正在停止...")
    reader.stop()

if __name__ == "__main__":
    # 配置参数（根据实际情况修改）
    PORT = "/dev/ttyUSB0"  # 串口端口号
    SENSOR_TYPE = "D80"  # 传感器类型
    
    # 创建XJCSensorReader实例
    reader = XJCSensorReader(PORT, sensor_type=SENSOR_TYPE)
    
    # 注册信号处理函数，捕获Ctrl+C信号
    signal.signal(signal.SIGINT, signal_handler)
    
    # 打印启动提示信息
    print("力传感器数据采集启动（按Ctrl+C退出）")
    
    # 启动数据采集
    reader.start()